from __future__ import annotations

import asyncio
import json
import math
import re
import time
from typing import Dict, Any
from gglobal import logger

from PySide2.QtCore import QThread, Signal


class WebSocketBridgeClient(QThread):
    """WebSocket桥接客户端（在QThread中运行，解析车辆状态并通过信号通知UI）。"""

    # 状态变化信号：当车辆状态字典发生变化且满足刷新间隔时触发
    status_changed = Signal(dict)
    # 连接状态信号：用于展示连接/断开等信息（UI可选使用）
    connection_status = Signal(bool, str)
    # 错误信号：网络错误、解析错误等
    error = Signal(str)

    def __init__(self, ip_address: str, parent=None) -> None:
        super().__init__(parent)
        # 服务器地址（参考测试代码），端口与路径可按需调整
        self.server_url = f"ws://{ip_address}:9090/"
        # 订阅的话题列表（参考测试代码）
        self.topics = [
            {"topic": "/velocity_planner/nearest_obstacle", "type": "std_msgs/String", "throttle_rate": 0},
            {"topic": "/vel_mb", "type": "geometry_msgs/Twist", "throttle_rate": 0},
            {"topic": "/monitor_errorcode", "type": "std_msgs/String", "throttle_rate": 0},
            {"topic": "/info_back", "type": "std_msgs/String", "throttle_rate": 0},
        ]

        # 内部运行控制
        self._stop_flag: bool = False
        self._last_emit_ts: float = 0.0  # 控制信号触发频率（约10fps）

        # 车辆状态字典（仅保存最新值，避免内存堆积）
        self._status: Dict[str, Any] = {
            "robotid": None,
            "robotverion": None,
            "robotspeed": 0.0,
            "obs": None,
            "charge": None,
        }

    # ---------------- 线程与运行控制 ----------------
    def stop(self) -> None:
        """请求停止客户端（线程安全标志位）。"""
        self._stop_flag = True
        try:
            logger.info("收到停止WebSocket桥接客户端请求")
        except Exception:
            pass

    def run(self) -> None:
        """线程入口：运行异步事件循环。"""
        try:
            asyncio.run(self._run())
        except Exception as exc:
            try:
                self.error.emit(f"运行错误: {exc}")
            except Exception:
                pass

    async def _run(self) -> None:
        """异步主循环：连接、订阅、接收并解析消息。"""
        try:
            import websockets
        except Exception:
            self.error.emit("未安装 websockets 库，请在conda base环境安装：pip install websockets")
            try:
                logger.error("websockets 库未安装")
            except Exception:
                pass
            return

        while not self._stop_flag:
            try:
                async with websockets.connect(
                    self.server_url,
                    ping_interval=20,
                    ping_timeout=20,
                    max_size=None,
                ) as ws:
                    # 通知连接成功
                    try:
                        self.connection_status.emit(True, "connected")
                        logger.info(f"WebSocket已连接: {self.server_url}")
                    except Exception:
                        pass

                    # 设置JSON协议并订阅话题
                    await ws.send(json.dumps({"op": "protocol", "data": "json"}))
                    for t in self.topics:
                        await ws.send(self._build_subscribe_msg(t))
                    try:
                        logger.info(f"已订阅话题: {[x['topic'] for x in self.topics]}")
                    except Exception:
                        pass

                    # 接收循环
                    async for raw in ws:
                        if self._stop_flag:
                            break
                        try:
                            if isinstance(raw, (bytes, bytearray)):
                                # 简化：忽略二进制格式，避免额外依赖
                                continue
                            msg = json.loads(raw)
                        except Exception as parse_exc:
                            # 解析失败，不中断主循环
                            self._safe_emit_error(f"解析错误: {parse_exc}")
                            try:
                                logger.debug(f"解析错误: {parse_exc}")
                            except Exception:
                                pass
                            continue

                        if msg.get("op") == "publish":
                            topic = msg.get("topic", "")
                            data = msg.get("msg")
                            self._dispatch(topic, data)

                    # 连接自然结束
                    try:
                        self.connection_status.emit(False, "disconnected")
                        logger.info("WebSocket连接已结束")
                    except Exception:
                        pass

            except Exception as conn_exc:
                # 网络错误：通知并重试
                self._safe_emit_error(f"连接失败或中断: {conn_exc}")
                try:
                    logger.warning(f"连接失败或中断: {conn_exc}")
                except Exception:
                    pass
                try:
                    self.connection_status.emit(False, "retry")
                except Exception:
                    pass
                await asyncio.sleep(3)

    # ---------------- 消息分派与解析 ----------------
    def _dispatch(self, topic: str, payload: Any) -> None:
        """根据topic分派至对应解析函数。"""
        try:
            if topic == "/vel_mb":
                self._handle_vel_mb(payload)
            elif topic == "/velocity_planner/nearest_obstacle":
                s = (payload or {}).get("data")
                self._handle_nearest_obstacle(s)
            elif topic == "/monitor_errorcode":
                s = (payload or {}).get("data")
                self._set_status("errorcode", s)
            elif topic == "/info_back":
                s = (payload or {}).get("data")
                self._handle_info_back(s)
        except Exception as exc:
            self._safe_emit_error(f"处理topic错误({topic}): {exc}")

    def _handle_vel_mb(self, msg: Dict[str, Any]) -> None:
        """解析速度数据：计算前进合速度与Yaw角速度（参考测试代码）。"""
        linear = (msg or {}).get("linear", {})
        angular = (msg or {}).get("angular", {})
        try:
            lx = float(linear.get("x", 0.0) or 0.0)
            ly = float(linear.get("y", 0.0) or 0.0)
        except Exception:
            lx = ly = 0.0
        # 使用±号表示方向：基于前进方向分量lx确定符号，大小为合速度
        mag = math.hypot(lx, ly)
        if mag < 1e-6:
            signed_speed = 0.0
        else:
            signed_speed = mag if lx >= 0.0 else -mag
        self._set_status("robotspeed", signed_speed)

    def _handle_nearest_obstacle(self, data: str | None) -> None:
        """解析最近障碍信息（参考测试代码）。"""
        s = str(data or "").strip()
        if not s or s == "---" or ("no obstacle" in s.lower()):
            self._set_status("obs", None)
            return
        m = re.search(r"id:(\d+),type:(\d+),src:(\d+),static:(\d+),fw:(\d+)", s)
        if not m:
            # 未命中结构化信息，直接展示原始片段（避免丢失）
            self._set_status("obs", s)
            return
        type_value = int(m.group(2))
        src_value = int(m.group(3))
        static_value = int(m.group(4))
        fw_value = int(m.group(5))

        type_meaning = {1: "静态", 2: "动态", 3: "交管"}.get(type_value, "未知")
        static_meaning = "是" if static_value == 1 else "否"
        fw_meaning = "是" if fw_value == 1 else "否"

        src_meanings = {
            0: "下激光",
            1: "前相机",
            2: "后realsense相机",
            3: "前mid360",
            4: "32线顶激光",
            5: "后mid360",
            6: "16线顶激光",
            7: "reasence_rgb",
            8: "叉臂单线激光",
        }
        src_sources = [name for bit, name in src_meanings.items() if (src_value & (1 << bit))]
        log_message = f"Type: {type_meaning}, Src: {', '.join(src_sources)}, Static: {static_meaning}, FW: {fw_meaning}"
        self._set_status("obs", log_message)

    def _handle_info_back(self, data: str | None) -> None:
        """解析综合信息：提取车辆ID、版本与电量（参考测试代码）。"""
        s = str(data or "")
        info: Dict[str, Any] = {}
        try:
            for line in s.splitlines():
                line = line.strip()
                if not line:
                    continue
                m_ver = re.search(r"CMD_VERSION:\s*(\{.*\})", line)
                if m_ver:
                    try:
                        ver = json.loads(m_ver.group(1))
                        info.update(ver)
                    except Exception:
                        pass
                    continue
                m_charge = re.search(r"CMD_GETCHARGE:\s*(\d+)", line)
                if m_charge:
                    info["CMD_GETCHARGE"] = int(m_charge.group(1))
                    continue
        except Exception as exc:
            self._safe_emit_error(f"/info_back解析错误: {exc}")

        # 更新关键信息（仅当存在时）
        if "robotid" in info:
            self._set_status("robotid", info.get("robotid"))
        if "robotverion" in info:
            self._set_status("robotverion", info.get("robotverion"))
        if "CMD_GETCHARGE" in info:
            self._set_status("charge", info.get("CMD_GETCHARGE"))

    # ---------------- 工具与变更检测 ----------------
    def _build_subscribe_msg(self, topic_conf: Dict[str, Any]) -> str:
        """构建订阅消息为JSON字符串。"""
        payload = {
            "op": "subscribe",
            "topic": topic_conf["topic"],
            "type": topic_conf["type"],
            "throttle_rate": topic_conf.get("throttle_rate", 0),
        }
        return json.dumps(payload)

    def _set_status(self, key: str, value: Any) -> None:
        """仅在值变化时更新并触发信号，避免不必要的UI刷新。"""
        try:
            old = self._status.get(key)
            if old != value:
                self._status[key] = value
                self._emit_status_changed_throttled()
                # 轻量日志：记录变化键
                try:
                    logger.debug(f"状态更新: {key} -> {value}")
                except Exception:
                    pass
        except Exception:
            # 忽略非关键错误，保持运行
            pass

    def _emit_status_changed_throttled(self) -> None:
        """按频率限制触发状态变化信号（约10fps，延迟不超过500ms）。"""
        now = time.monotonic()
        if (now - self._last_emit_ts) >= 0.1:
            self._last_emit_ts = now
            try:
                self.status_changed.emit(dict(self._status))
                logger.debug("已发出状态变化信号")
            except Exception:
                pass

    def _safe_emit_error(self, message: str) -> None:
        """安全发出错误信号（避免异常中断主流程）。"""
        try:
            self.error.emit(message)
            logger.error(message)
        except Exception:
            pass